from machine import I2C, Pin
import time

MPU_ADDR = 0X68

MPU_DEVICE_ID_REG = 0x75
MPU_PWR_MGMT1_REG = 0x6B
MPU_PWR_MGMT2_REG = 0x6C
MPU_SELF_TESTX_REG = 0x0D
MPU_SELF_TESTY_REG = 0x0E
MPU_SELF_TESTZ_REG = 0x0F
MPU_SELF_TESTA_REG = 0x10
MPU_SAMPLE_RATE_REG = 0x19
MPU_CFG_REG = 0x1A
MPU_GYRO_CFG_REG = 0x1B
MPU_ACCEL_CFG_REG = 0x1C
MPU_MOTION_DET_REG = 0x1F
MPU_FIFO_EN_REG = 0x23
MPU_I2CMST_CTRL_REG = 0x24
MPU_I2CSLV0_ADDR_REG = 0x25
MPU_I2CSLV0_REG = 0x26
MPU_I2CSLV0_CTRL_REG = 0x27
MPU_I2CSLV1_ADDR_REG = 0x28
MPU_I2CSLV1_REG = 0x29
MPU_I2CSLV1_CTRL_REG = 0x2A
MPU_I2CSLV2_ADDR_REG = 0x2B
MPU_I2CSLV2_REG = 0x2C
MPU_I2CSLV2_CTRL_REG = 0x2D
MPU_I2CSLV3_ADDR_REG = 0x2E
MPU_I2CSLV3_REG = 0x2F
MPU_I2CSLV3_CTRL_REG = 0x30
MPU_I2CSLV4_ADDR_REG = 0x31
MPU_I2CSLV4_REG = 0x32
MPU_I2CSLV4_DO_REG = 0x33
MPU_I2CSLV4_CTRL_REG = 0x34
MPU_I2CSLV4_DI_REG = 0x35

MPU_I2CMST_STA_REG = 0x36
MPU_INTBP_CFG_REG = 0x37
MPU_INT_EN_REG = 0x38
MPU_INT_STA_REG = 0x3A

MPU_ACCEL_XOUTH_REG = 0x3B
MPU_ACCEL_XOUTL_REG = 0x3C
MPU_ACCEL_YOUTH_REG = 0x3D
MPU_ACCEL_YOUTL_REG = 0x3E
MPU_ACCEL_ZOUTH_REG = 0x3F
MPU_ACCEL_ZOUTL_REG = 0x40

MPU_TEMP_OUTH_REG = 0x41
MPU_TEMP_OUTL_REG = 0x42

MPU_GYRO_XOUTH_REG = 0x43
MPU_GYRO_XOUTL_REG = 0x44
MPU_GYRO_YOUTH_REG = 0x45
MPU_GYRO_YOUTL_REG = 0x46
MPU_GYRO_ZOUTH_REG = 0x47
MPU_GYRO_ZOUTL_REG = 0x48

MPU_I2CSLV0_DO_REG = 0x63
MPU_I2CSLV1_DO_REG = 0x64
MPU_I2CSLV2_DO_REG = 0x65
MPU_I2CSLV3_DO_REG = 0x66

MPU_I2CMST_DELAY_REG = 0x67
MPU_SIGPATH_RST_REG = 0x68
MPU_MDETECT_CTRL_REG = 0x69
MPU_USER_CTRL_REG = 0x6A
MPU_PWR_MGMT1_REG = 0x6B
MPU_PWR_MGMT2_REG = 0x6C
MPU_FIFO_CNTH_REG = 0x72
MPU_FIFO_CNTL_REG = 0x73
MPU_FIFO_RW_REG = 0x74
MPU_DEVICE_ID_REG = 0x75

MPU_ADDR_ADDR = 0x68


class MPU6050(object):
    def __init__(self, sclIO, sdaIO):
        self.i2c = I2C(scl=Pin(sclIO), sda=Pin(sdaIO), freq=100000)

    def Write_Mpu6050_REG(self, reg, dat):
        buf = bytearray(1)
        buf[0] = dat
        self.i2c.writeto_mem(MPU_ADDR, reg, buf)

    def Read_Mpu6050_REG(self, reg):
        t = self.i2c.readfrom_mem(MPU_ADDR, reg, 1)[0]
        return (t >> 4) * 10 + (t % 16)

    def Read_Mpu6050_Len(self, reg, len, buffer):
        # buffer=bytearray(len)
        self.i2c.readfrom_mem_into(MPU_ADDR, reg, buffer)

    # fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
    def MPU_Set_Gyro_Fsr(self, fsr):
        return self.Write_Mpu6050_REG(MPU_GYRO_CFG_REG, fsr << 3)

    # fsr:0,±2g;1,±4g;2,±8g;3,±16g
    def MPU_Set_Accel_Fsr(self, fsr):
        return self.Write_Mpu6050_REG(MPU_ACCEL_CFG_REG, fsr << 3)

    def MPU_Set_LPF(self, lpf):
        if (lpf >= 188):
            data = 1
        elif (lpf >= 98):
            data = 2
        elif (lpf >= 42):
            data = 3
        elif (lpf >= 20):
            data = 4
        elif (lpf >= 10):
            data = 5
        else:
            data = 6;
        self.Write_Mpu6050_REG(MPU_CFG_REG, data)

    # rate:4~1000(Hz)
    def MPU_Set_Rate(self, rate):
        if (rate > 1000):
            rate = 1000
        if (rate < 4):
            rate = 4;
        data = int(1000 / rate - 1)
        datas = self.Write_Mpu6050_REG(MPU_SAMPLE_RATE_REG, data)
        return self.MPU_Set_LPF(rate / 2)

    def MPU_Init(self):
        self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG, 0x80)
        time.sleep_ms(100)
        self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG, 0x00)
        self.MPU_Set_Gyro_Fsr(3)
        self.MPU_Set_Accel_Fsr(0)
        self.MPU_Set_Rate(50)
        self.Write_Mpu6050_REG(MPU_INT_EN_REG, 0x00)
        self.Write_Mpu6050_REG(MPU_USER_CTRL_REG, 0x00)
        self.Write_Mpu6050_REG(MPU_FIFO_EN_REG, 0x00)
        self.Write_Mpu6050_REG(MPU_INTBP_CFG_REG, 0x80)
        res = self.Read_Mpu6050_REG(MPU_DEVICE_ID_REG)
        if (res == 68):
            self.Write_Mpu6050_REG(MPU_PWR_MGMT1_REG, 0x01)
            self.Write_Mpu6050_REG(MPU_PWR_MGMT2_REG, 0x00)
            self.MPU_Set_Rate(50)
        else:
            return 1
        return 0

    # Get raw data
    def GetGyro(self):
        buf = bytearray(6)
        res = self.Read_Mpu6050_Len(MPU_GYRO_XOUTH_REG, 6, buf)
        gx = (buf[0] << 8) | buf[1]
        gy = (buf[2] << 8) | buf[3]
        gz = (buf[4] << 8) | buf[5]
        # print('MPU_Get_Gyroscope: ',gx,gy,gz)
        return gx, gy, gz

    def GetAccel(self):
        buf = bytearray(6)
        res = self.Read_Mpu6050_Len(MPU_ACCEL_XOUTH_REG, 6, buf)
        ax = (buf[0] << 8) | buf[1]
        ay = (buf[2] << 8) | buf[3]
        az = (buf[4] << 8) | buf[5]
        # print('MPU_Get_Accelerometer: ',ax,ay,az)
        return ax, ay, az